#!/usr/bin/python
# coding=utf-8
import time
import rospy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Image
from std_msgs.msg import Int8


class visoin_adjust_clean:
    def __init__(self):
        self.cmd_pub = rospy.Publisher(
            "/cmd_vel", Twist, queue_size=10)			# 发布运动控制信息
        self.moveclean_cmd = rospy.Publisher(
            "/moveclean_cmd", Int8, queue_size=10)
        self.clean_cmd = rospy.Publisher("/clean_cmd", Int8, queue_size=10)
        self.moveclean_report_sub = rospy.Subscriber(
            "/moveclean_report", Int8, self.moveCleanReportCallback)
        self.clean_report_sub = rospy.Subscriber(
            "/clean_report", Int8, self.cleanReportCallback)
        self.vision_sub = rospy.Subscriber('/darknet_ros/bounding_boxes', BoundingBoxes, self.vision_callback, queue_size=10)
        self.start_clean_flag = True
        self.finish_clean_flag = True

    # 0 normal,1 get start ironing,2 get start cleaning, 3 finish cleaning,
    # 4 get force stop cleaning, 5 request stop moving, 6 request moving
    def cleanReportCallback(self, msg):
        report_clean_flag = msg.data
        rospy.loginfo("cleanReportCallback report_clean_flag :"+str(report_clean_flag))
        if report_clean_flag == 2:  # 清洁板收到开始清洁命令
            self.start_clean_flag = False
        elif report_clean_flag == 5:  # 清洁板请求停止运动
            twist = Twist()
            twist.linear.x = 0
            twist.angular.z = 0
            self.cmd_pub.publish(twist)
        elif report_clean_flag == 6:  # 清洁板请求运动
            twist = Twist()
            twist.linear.x = 0.02
            twist.angular.z = 0
            self.cmd_pub.publish(twist)
        elif report_clean_flag == 3:  # 清洁板上报清洁结束
            if self.finish_clean_flag:
                i = 0
                while(1):
                    time.sleep(1)  # 刘鹤要求
                    clean_flag = Int8()
                    moveclean_flag = Int8()
                    clean_flag.data = 5  # 3 forward cleaning, 4 reverse cleaning, 5 stop cleaning
                    moveclean_flag.data = 3  # 2 start clean,3 stop clean
                    self.clean_cmd.publish(clean_flag)
                    self.moveclean_cmd.publish(moveclean_flag)
                    rospy.loginfo("finish clean")
                    twist = Twist()
                    twist.linear.x = 0
                    twist.angular.z = 0
                    self.cmd_pub.publish(twist)
                    # self.finish_clean_flag = False
                    i += 1
                    if(i == 1):
                        break

    def moveCleanReportCallback(self, msg):
        moveclean_report_status = msg.data
        rospy.loginfo("moveclean_report_status: "+str(moveclean_report_status))
        moveclean_flag = Int8()
        clean_flag = Int8()
        moveclean_flag.data = 0  
        clean_flag.data = 0
        if moveclean_report_status == 1:  # 控制板收到开始对位
            start_adjust_flag = False
        elif moveclean_report_status == 2:  # 控制板发送对位结束，开始清洁
            if self.start_clean_flag:
                # i = 0
                # while(1):
                time.sleep(0.5)  # 刘鹤要求
                moveclean_flag.data = 2  # start clean 0x27
                clean_flag.data = 3
                # 2 start clean,3 stop clean
                self.moveclean_cmd.publish(moveclean_flag)
                # 3 forward cleaning, 4 reverse cleaning, 5 stop cleaning
                self.clean_cmd.publish(clean_flag)
                rospy.loginfo("finish adjust and start clean")
                    # i += 1
                    # if(i == 1):
                    #     break
        elif moveclean_report_status == 3:  # 控制板收到开始清洁
            self.start_clean_flag = False
        elif moveclean_report_status == 4:  # 控制板收到开始清洁结束
            self.finish_clean_flag = False
    
    def vision_callback(self,data):
        object_list = ["sports ball","cup","bottle"]
        for box in data.bounding_boxes:
            self.moveclean_cmd.publish(5)
            twist = Twist()
            twist.linear.x = 0
            twist.angular.z = 0
            self.cmd_pub.publish(twist)

if __name__ == '__main__':
    try:
        rospy.init_node('adjust_clean_node',anonymous=True)
        ac = visoin_adjust_clean()
        rospy.spin()
    except KeyboardInterrupt:
        ac.serial.close
        rospy.loginfo("adjust_clean_node shutting down")
